#include "../H/Elevator.h"

//------------------------------------------------------------------------------
// METHOD:  Elevator::Elevator()
// Type:	Constructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
Elevator::Elevator()
{
	static const INT8   DIGITAL_SIDECAR_MODULE = 1;

	static const INT32  ELEVATOR_ENCODER_CHA   = 11;
	static const INT32  ELEVATOR_ENCODER_CHB   = 12;

	static const bool   ENCODER_NORMAL_DIR         = true;
//	static const bool   ENCODER_REVERSE_DIR        = false;

	static const INT32  ELEVATOR_MOTOR_CH      = 10;

	// NOT SURE IF THIS IS NEEDED - MAY USE JUST ENCODER COUNT
    // ALSO NEED TO DETERMINE IF WE NEED TO INITIALIZE THE ENCODER
    // (RESET COUNT TO ZERO


	pElevatorMotor  = new Talon(DIGITAL_SIDECAR_MODULE,ELEVATOR_MOTOR_CH);
	pElevatorHeight = new Encoder(ELEVATOR_ENCODER_CHA,ELEVATOR_ENCODER_CHB
			                     ,ENCODER_NORMAL_DIR);

	elevatorAtTarget = false;
	savedHeight      = 0;     // Set to encoder reading of stow position
}
//------------------------------------------------------------------------------
// METHOD:  Elevator::~Elevator()
// Type:	Destructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
Elevator::~Elevator()
{
}
//------------------------------------------------------------------------------
// METHOD:  Elevator::MoveElevator()
// Type:	Public accessor method
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
bool Elevator::MoveElevator(float elevDSPos)
{
	INT32  elevEncdrTarget;

	elevEncdrTarget = CalcElevatorTarget(elevDSPos);

	elevatorTargetDown    = elevEncdrTarget + DOWN_BUFFER;
	elevatorTargetUp      = elevEncdrTarget - UP_BUFFER;

	if ( elevEncdrTarget > savedHeight )
	{
		elevatorAtTarget = false;
		MoveElevatorUp();
	}
	else
	{
		if ( elevEncdrTarget < savedHeight )
		{
			elevatorAtTarget = false;
			MoveElevatorDown();
		}
		else
		{
			elevatorAtTarget = true;
		}
	}

	return elevatorAtTarget;
}
//------------------------------------------------------------------------------
// METHOD:  Elevator::CalcElevatorTarget()
// Type:	Destructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
INT32 Elevator::CalcElevatorTarget(float elevDSPos)
{
	INT32  elevEncdrTarget;

	elevEncdrTarget
		= (INT32 ( ( ELEV_CALC_RATIO * elevDSPos ) + ELEV_CALC_CONST ));

	return elevEncdrTarget;
}
//------------------------------------------------------------------------------
// METHOD:  Elevator::MoveElevatorUp()
// Type:	Destructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void Elevator::MoveElevatorUp()
{
	static const float FULL_SPEED_UP = 1.0;
	static const float STOP          = 0.0;

	if ( pElevatorHeight->Get() < elevatorTargetUp )
	{
		pElevatorMotor->Set(FULL_SPEED_UP);
	}
	else
	{
		pElevatorMotor->Set(STOP);
		savedHeight = newTargetHeight;
	}

	return;
}
//------------------------------------------------------------------------------
// METHOD:  Elevator::MoveElevatorDown()
// Type:	Destructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void Elevator::MoveElevatorDown()
{
	static const float FULL_SPEED_DOWN = 1.0;
	static const float STOP            = 0.0;

	if ( pElevatorHeight->Get() > elevatorTargetDown )
	{
		pElevatorMotor->Set(FULL_SPEED_DOWN);
	}
	else
	{
		pElevatorMotor->Set(STOP);
		savedHeight = newTargetHeight;
	}

	return;
}
//------------------------------------------------------------------------------
// METHOD:  Elevator::GetPosition()
// Type:	Destructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
double Elevator::GetPosition()
{
	return pElevatorHeight->GetDistance();
}

//------------------------------------------------------------------------------
// METHOD:  Elevator::GetPosition()
// Type:	Destructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void Elevator::SetDistancePerPulse(float DIST_PER_PULSE)
{
	pElevatorHeight->SetDistancePerPulse(DIST_PER_PULSE);
}

//------------------------------------------------------------------------------
// METHOD:  Elevator::ResetEncoders()
// Type:	Destructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void Elevator::ResetEncoders()
{
	pElevatorHeight->Reset();
}
